/*
 * Software License Agreement (BSD License)
 *
 * Copyright (c) 2011, Willow Garage, Inc.
 * All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions
 * are met:
 *
 *  * Redistributions of source code must retain the above copyright
 *    notice, this list of conditions and the following disclaimer.
 *  * Redistributions in binary form must reproduce the above
 *    copyright notice, this list of conditions and the following
 *    disclaimer in the documentation and/or other materials provided
 *    with the distribution.
 *  * Neither the name of Willow Garage, Inc. nor the names of its
 *    contributors may be used to endorse or promote prducts derived
 *    from this software without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 * POSSIBILITY OF SUCH DAMAGE.
 */

#ifndef ROS_NODE_HANDLE_H_
#define ROS_NODE_HANDLE_H_

#include <ros/std_msgs/Time.h>
#include <ros/rosserial_msgs/TopicInfo.h>
#include <ros/rosserial_msgs/Log.h>
#include <ros/rosserial_msgs/RequestParam.h>

#define SYNC_SECONDS        5

#define MODE_FIRST_FF       0
#define MODE_SECOND_FF      1
#define MODE_TOPIC_L        2   // waiting for topic id
#define MODE_TOPIC_H        3
#define MODE_SIZE_L         4   // waiting for message size
#define MODE_SIZE_H         5
#define MODE_MESSAGE        6
#define MODE_CHECKSUM       7

#define MSG_TIMEOUT 20  //20 milliseconds to recieve all of message data

#include <ros/node_output.h>

#include <ros/publisher.h>
#include <ros/msg_receiver.h>
#include <ros/subscriber.h>
#include <ros/service_server.h>

namespace ros
{

using rosserial_msgs::TopicInfo;

/* Node Handle */
template<class Hardware,
         int MAX_SUBSCRIBERS=25,
         int MAX_PUBLISHERS=25,
         int INPUT_SIZE=512,
         int OUTPUT_SIZE=512>
class NodeHandle_
{
protected:
  Hardware hardware_;
  NodeOutput<Hardware, OUTPUT_SIZE> no_;

  /* time used for syncing */
  unsigned long rt_time;

  /* used for computing current time */
  unsigned long sec_offset, nsec_offset;

  unsigned char message_in[INPUT_SIZE];

  Publisher * publishers[MAX_PUBLISHERS];
  MsgReceiver * receivers[MAX_SUBSCRIBERS];

  /*
   * Setup Functions
   */
public:
  NodeHandle_()
    : no_(&hardware_) {}

  Hardware* getHardware(void)
  { return &hardware_; }

  /* Start serial, initialize buffers */
  void initNode(void)
  {
    hardware_.init();
    mode_ = 0;
    bytes_ = 0;
    index_ = 0;
    topic_ = 0;
    total_receivers=0;
  }

protected:

  //State machine variables for spinOnce
  int mode_;
  int bytes_;
  int topic_;
  int index_;
  int checksum_;

  int total_receivers;

  /* used for syncing the time */
  unsigned long last_sync_time;
  unsigned long last_sync_receive_time;
  unsigned long last_msg_timeout_time;

  bool registerReceiver(MsgReceiver* rcv)
  {
    if (total_receivers >= MAX_SUBSCRIBERS)
      return false;
    receivers[total_receivers] = rcv;
    rcv->id_ = 100+total_receivers;
    total_receivers++;
    return true;
  }

public:
  /* This function goes in your loop() function, it handles
   * serial input and callbacks for subscribers.
   */

  virtual void spinOnce(void)
  {
    /* restart if timed out */
    unsigned long c_time = hardware_.time();
    if ((c_time - last_sync_receive_time) > (SYNC_SECONDS*2200))
    { no_.setConfigured(false); }

    /* reset if message has timed out */
    if (mode_ != MODE_FIRST_FF)
      if (c_time > last_msg_timeout_time)
        mode_ = MODE_FIRST_FF;

    /* while available buffer, read data */
    while(true)
    {
      int data = hardware_.read();
      if(data < 0)
        break;
      checksum_ += data;

      /* message data being recieved */
      if (mode_ == MODE_MESSAGE)
      {
        message_in[index_++] = data;
        bytes_--;

        /* is message complete? if so, checksum */
        if(bytes_ == 0)
          mode_ = MODE_CHECKSUM;
      }
      else if (mode_ == MODE_FIRST_FF)
      {
        if(data == 0xff)
        {
          mode_++;
          last_msg_timeout_time = c_time + MSG_TIMEOUT;
        }
      }
      else if (mode_ == MODE_SECOND_FF)
      {
        if(data == 0xff)
          mode_++;
        else
          mode_ = MODE_FIRST_FF;
      }
      /* bottom half of topic id */
      else if (mode_ == MODE_TOPIC_L)
      {
        topic_ = data;
        mode_++;
        checksum_ = data;                 /* first byte included in checksum */
      }
      /* top half of topic id */
      else if (mode_ == MODE_TOPIC_H)
      {
        topic_ += data<<8;
        mode_++;
      }
      /* bottom half of message size */
      else if (mode_ == MODE_SIZE_L)
      {
        bytes_ = data;
        index_ = 0;
        mode_++;
      }
      /* top half of message size */
      else if (mode_ == MODE_SIZE_H)
      {
        bytes_ += data<<8;
        mode_ = MODE_MESSAGE;
        if(bytes_ == 0)
          mode_ = MODE_CHECKSUM;
      }
      /* do checksum */
      else if (mode_ == MODE_CHECKSUM)
      {
        if ((checksum_%256) == 255)
        {
          if (topic_ == TopicInfo::ID_PUBLISHER)
          {
            requestSyncTime();
            negotiateTopics();
            last_sync_time = c_time;
            last_sync_receive_time = c_time;
          }
          else if (topic_ == TopicInfo::ID_TIME)
          { syncTime(message_in); }
          else if (topic_ == TopicInfo::ID_PARAMETER_REQUEST)
          {
            req_param_resp.deserialize(message_in);
            param_recieved= true;
          }
          else
          {
            if(receivers[topic_-100])
              receivers[topic_-100]->receive(message_in);
          }
        }
        mode_ = MODE_FIRST_FF;
      }
    }

    /* occasionally sync time */
    if (no_.configured() && ((c_time-last_sync_time) > (SYNC_SECONDS*500)))
    {
      requestSyncTime();
      last_sync_time = c_time;
    }
  }

  /* Are we connected to the PC? */
  bool connected(void)
  { return no_.configured(); }

  /*
   * Time functions
   */

  void requestSyncTime(void)
  {
    std_msgs::Time t;
    no_.publish(TopicInfo::ID_TIME, &t);
    rt_time = hardware_.time();
  }

  void syncTime(unsigned char * data)
  {
    std_msgs::Time t;
    unsigned long offset = hardware_.time() - rt_time;

    t.deserialize(data);

    t.data.sec += offset/1000;
    t.data.nsec += (offset%1000)*1000000UL;

    this->setNow(t.data);
    last_sync_receive_time = hardware_.time();
  }

  Time now(void)
  {
    unsigned long ms = hardware_.time();
    Time current_time;
    current_time.sec = ms/1000 + sec_offset;
    current_time.nsec = (ms%1000)*1000000UL + nsec_offset;
    normalizeSecNSec(current_time.sec, current_time.nsec);
    return current_time;
  }

  void setNow(Time & new_now)
  {
    unsigned long ms = hardware_.time();
    sec_offset = new_now.sec - ms/1000 - 1;
    nsec_offset = new_now.nsec - (ms%1000)*1000000UL + 1000000000UL;
    normalizeSecNSec(sec_offset, nsec_offset);
  }

  /*
   * Registration
   */

  bool advertise(Publisher & p)
  {
    int i;
    for(i = 0; i < MAX_PUBLISHERS; i++)
    {
      if(publishers[i] == 0) // empty slot
      {
        publishers[i] = &p;
        p.id_ = i+100+MAX_SUBSCRIBERS;
        p.no_ = &this->no_;
        return true;
      }
    }
    return false;
  }

  /* Register a subscriber with the node */
  template<typename MsgT>
  bool subscribe(Subscriber< MsgT> &s)
  { return registerReceiver((MsgReceiver*) &s); }

  template<typename SrvReq, typename SrvResp>
  bool advertiseService(ServiceServer<SrvReq,SrvResp>& srv)
  {
    srv.no_ = &no_;
    return registerReceiver((MsgReceiver*) &srv);
  }

  void negotiateTopics(void)
  {
    no_.setConfigured(true);

    rosserial_msgs::TopicInfo ti;
    int i;
    for(i = 0; i < MAX_PUBLISHERS; i++)
    {
      if(publishers[i] != 0) // non-empty slot
      {
        ti.topic_id = publishers[i]->id_;
        ti.topic_name = (char *) publishers[i]->topic_;
        ti.message_type = (char *) publishers[i]->msg_->getType();
        no_.publish(TopicInfo::ID_PUBLISHER, &ti);
      }
    }
    for(i = 0; i < MAX_SUBSCRIBERS; i++)
    {
      if(receivers[i] != 0) // non-empty slot
      {
        ti.topic_id = receivers[i]->id_;
        ti.topic_name = (char *) receivers[i]->topic_;
        ti.message_type = (char *) receivers[i]->getMsgType();
        no_.publish(TopicInfo::ID_SUBSCRIBER, &ti);
      }
    }
  }

  /*
   * Logging
   */

private:
  void log(char byte, const char * msg)
  {
    rosserial_msgs::Log l;
    l.level= byte;
    l.msg = (char*)msg;
    this->no_.publish(rosserial_msgs::TopicInfo::ID_LOG, &l);
  }

public:
  void logdebug(const char* msg)
  { log(rosserial_msgs::Log::DEBUG, msg); }

  void loginfo(const char * msg)
  { log(rosserial_msgs::Log::INFO, msg); }

  void logwarn(const char *msg)
  { log(rosserial_msgs::Log::WARN, msg); }

  void logerror(const char*msg)
  { log(rosserial_msgs::Log::ERROR, msg); }

  void logfatal(const char*msg)
  { log(rosserial_msgs::Log::FATAL, msg); }

  /*
   * Retrieve Parameters
   */

private:

  bool param_recieved;
  rosserial_msgs::RequestParamResponse req_param_resp;

  bool requestParam(const char * name, int time_out =  1000)
  {
    param_recieved = false;
    rosserial_msgs::RequestParamRequest req;
    req.name  = (char*)name;
    no_.publish(TopicInfo::ID_PARAMETER_REQUEST, &req);
    int end_time = hardware_.time();

    while(!param_recieved)
    {
      spinOnce();
      if (end_time > hardware_.time()) return false;
    }
    return true;
  }

public:

  bool getParam(const char* name, int* param, int length =1)
  {
    if (requestParam(name))
    {
      if (length == req_param_resp.ints_length)
      {
        //copy it over
        for(int i=0; i<length; i++)
          param[i] = req_param_resp.ints[i];
        return true;
      }
    }
    return false;
  }

  bool getParam(const char* name, float* param, int length=1)
  {
    if (requestParam(name))
    {
      if (length == req_param_resp.floats_length)
      {
        //copy it over
        for(int i=0; i<length; i++)
          param[i] = req_param_resp.floats[i];
        return true;
      }
    }
    return false;
  }

  bool getParam(const char* name, char** param, int length=1)
  {
    if (requestParam(name))
    {
      if (length == req_param_resp.strings_length)
      {
        //copy it over
        for(int i=0; i<length; i++)
          strcpy(param[i],req_param_resp.strings[i]);
        return true;
      }
    }
    return false;
  }
};

} // ros

#endif // ROS_NODE_HANDLE_H_
